#include <cblas.h>
#include <lapacke.h>
#include "ConditionNumber.h"

Real estimateSquareCond(const Tensor<Real,2> &cA)
{
  assert(cA.size()[0] == cA.size()[1]);
  Tensor<Real,2> A = cA.copy(); // storage for the LU factorization
  const int N = A.size()[0];
  Tensor<int,1> ipiv(N);
  Real rcond;
  Real onenorm = LAPACKE_dlange(LAPACK_COL_MAJOR, '1', N, N, A.data(), N); // calculate the 1-norm
  LAPACKE_dgetrf(LAPACK_COL_MAJOR, N, N, A.data(), N, ipiv.data());        // the LU factorization
  LAPACKE_dgecon(LAPACK_COL_MAJOR, '1', N, A.data(), N, onenorm, &rcond);
  return 1.0 / rcond;
}

Real estimateUnderdetCond(const Tensor<Real,2> &cA)
{
  Tensor<Real,2> A(cA); // alias
  const int nBasis = cA.size()[0];
  const int nSources = cA.size()[1];
  assert(nSources >= nBasis);
  Tensor<Real,2> AAT(Vec<int,2> {nBasis, nBasis});
  Tensor<int,1> ipiv(nBasis);
  Real rcond;
  cblas_dgemm(CblasColMajor,
              CblasNoTrans/*TRANSA*/, CblasTrans/*TRANSB*/, nBasis/*m*/, nBasis/*n*/, nSources/*k*/,
              1.0/*alpha*/, A.data(), nBasis/*lda*/, A.data()/*B*/, nBasis/*ldb*/, 0.0/*beta*/,
              AAT.data()/*C*/, nBasis/*ldc*/);
  Real onenorm = LAPACKE_dlange(LAPACK_COL_MAJOR, '1', nBasis, nBasis, AAT.data(), nBasis); // calculate the 1-norm
  LAPACKE_dgetrf(LAPACK_COL_MAJOR, nBasis, nBasis, AAT.data(), nBasis, ipiv.data()); // the LU factorization
  LAPACKE_dgecon(LAPACK_COL_MAJOR, '1', nBasis, AAT.data(), nBasis, onenorm, &rcond);
  return 1.0 / sqrt(rcond);
}
